#include <ros/ros.h>
#include <map_study/map_pub.h>

class MapColor{
    public:
        MapColor();
        ~MapColor() = default;

    private:
        std::shared_ptr<MapPublish> map_pub_ptr_;
};


MapColor::MapColor(){
    map_pub_ptr_ = std::make_shared<MapPublish>("/map_color");

    nav_msgs::OccupancyGrid map_msg;
    map_msg.info.resolution = 0.05;
    map_msg.info.width = 256;
    map_msg.info.height = 20;
    map_msg.info.origin.position.x = -5.0;
    map_msg.info.origin.position.y = -0.2;
    map_msg.info.origin.position.z = 0.0;
    map_msg.info.origin.orientation.w = 1.0;
    map_msg.header.frame_id = "map";
    map_msg.header.stamp = ros::Time::now();

    for(int j = 0; j < map_msg.info.height; j++){
        for(int i = 0; i < map_msg.info.width; i++){
            // int8_t data = -128 + i;
            int8_t data = 0;
            map_msg.data.push_back(data);
        }
    }
    map_pub_ptr_->publishMap(map_msg);
}

int main(int argc, char** argv){
    ros::init(argc, argv, "map_color_node");
    MapColor map_color;
    ros::spin();
    return 0;
}